/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : mixer.c
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "main.h"



int32_t pTerm, dTerm;
int16_t iTerm;
int32_t sumval;
static int32_t lastPTerm[MAX_KPID];
static int32_t lastDTerm[MAX_KPID];
static int16_t errorIntegral[MAX_KPID];

int16_t outVal[TOTAL_SOURCES];
int16_t prevVal[TOTAL_SOURCES];
uint8_t switchVal[NUM_SWITCHES];

void calcPID()
{
    for(uint8_t ax=0; ax<MAX_KPID; ax++)
    {
        pTerm  = outVal[ax];// get PID values that were put in place by the premixer
        iTerm = errorIntegral[ax] = CONSTRAIN(errorIntegral[ax] + pTerm, 20000); // add error to integral and constrain
        dTerm = ((pTerm - lastPTerm[ax]) + lastDTerm[ax]) >> 1; //LP filter on dTerm

        lastDTerm[ax] = dTerm;
        lastPTerm[ax] = pTerm;

        sumval  = pTerm * (int32_t)eeData.kpid[ax].kp; //multiply by Kp - either stored or pwmIn if the value is negative
        sumval += iTerm * (int32_t)eeData.kpid[ax].ki;
        sumval += dTerm * (int32_t)eeData.kpid[ax].kd;
        sumval >>= KPID_DEVISOR;   //shift divide

        outVal[ax] = (int16_t)sumval; //feed back values to PID var
    }
}


int16_t intpol(int16_t x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
{
#define D9 (RESX * 2 / 8)
#define D5 (RESX * 2 / 4)
    bool    cv9 = idx >= MAX_CURVE5;
    int8_t *crv = cv9 ? eeData.curves9[idx-MAX_CURVE5] : eeData.curves5[idx];
    int16_t erg;

    x+=RESXu;
    if(x < 0) {
        erg = (int16_t)crv[0] * (RESX/4);
    } else if(x >= (RESX*2)) {
        erg = (int16_t)crv[(cv9 ? 8 : 4)] * (RESX/4);
    } else {
        int16_t a,dx;
        if(cv9){
            a   = (uint16_t)x / D9;
            dx  =((uint16_t)x % D9) * 2;
        } else {
            a   = (uint16_t)x / D5;
            dx  = (uint16_t)x % D5;
        }
        erg  = (int16_t)crv[a]*((D5-dx)/2) + (int16_t)crv[a+1]*(dx/2);
    }
    return erg / 25; // 100*D5/RESX;
}

int16_t initVal(uint8_t typ, uint8_t num)
{
    int16_t v = 0;
    switch (typ) {
    case (SRC_TYPE_CMD):
        if(num<MAX_CHN) v = pwmIn(num );
        break;
    case (SRC_TYPE_PID):
        if(num<MAX_KPID) v = outVal[num];  //do we use outVal or prevVal? - Using outVal here since it can only be used in the post-mixer, PID values cannot be used as inputs for PID outputs
        break;
    case (SRC_TYPE_CHN):
        if(num<MAX_CHN) v = prevVal[num];  //do we use outVal or prevVal? - Using previous value here
        break;
    case (SRC_TYPE_GYRO):
        if(num<3) v = sensor_data_int[SEN_GX + num];
        break;
    case (SRC_TYPE_ACCEL):
        if(num<3) v = sensor_data_int[SEN_AX + num];
        break;
    case (SRC_TYPE_MAG):
        if(num<3) v = sensor_data_int[SEN_MX + num];
        break;
    case (SRC_TYPE_BARO):
        v = sensor_data_int[SEN_B];
        break;
    case (SRC_TYPE_ATTITUDE):
        if(num<3) v = attitudeCMD[num];
        break;
    case (SRC_TYPE_AP):
        v = 0; //AP not defined yet
        break;
    default:
    case (SRC_TYPE_CONST):
        v = RESX; //returns equvalent of 1
        break;
    }

    return v;
}

void doMix(uint8_t idx) //perform mix #idx
{

    MixData *md = &eeData.mixData[idx];

    if((md->destCh==0) || (md->destCh>NUM_XCHNOUT)) return;

    int16_t v = initVal(md->srcType, md->srcNum);    //get initial value into v
    v += md->sOffset; //add offset to it

    //========== DELAY and PAUSE ===============


    //========== CURVES ===============
    switch(md->curve){
    case 0:
        break;
    case 1:
        if( v<0 ) v=0;   //x|x>0
        break;
    case 2:
        if( v>0 ) v=0;   //x|x<0
        break;
    case 3:       // x|abs(x)
        v = abs(v);
        break;
    case 4:       //f|f>0
        v = v>0 ? RESX : 0;
        break;
    case 5:       //f|f<0
        v = v<0 ? -RESX : 0;
        break;
    case 6:       //f|abs(f)
        v = v>0 ? RESX : -RESX;
        break;
    default: //c1..c16
        v = intpol(v, md->curve - 7);
    }


    //========== MULTIPLEX ===============
    v = ((int32_t)v*md->weight) >> 8; //multiply by weight and divide by 256
    int32_t dv = v;
    switch((uint8_t)md->mltpx) {
    case MLTPX_REP:
        outVal[md->destCh-1] = v;
        break;
    case MLTPX_MUL:
        dv *= outVal[md->destCh-1];
        dv >>= RESXsh; //divide by RESX -> fixed point math
        outVal[md->destCh-1] = dv;
        break;
    default:  // MLTPX_ADD
        outVal[md->destCh-1] += v;
        break;
    }
}

uint8_t getSwitch(uint8_t sw) //calculate switch value
{
    return sw | 1; //make sure it's on - until I finish this
}


void mixer()
{
    //pre-populate switches so we use current state of vars
    for(uint8_t i=0; i<NUM_SWITCHES; i++)
        switchVal[i] = getSwitch(i);

    memcpy(&prevVal, &outVal, sizeof(prevVal)); // copy previous values
    memset(&outVal, 0, sizeof(outVal)); //zero current output

    uint8_t idx=0;

    //premixer - calculate only PID outputs    
    while (idx<MAX_MIXERS)
    {
        if((!eeData.mixData[idx].destCh) || (eeData.mixData[idx].destCh>=MAX_KPID)) //if no more mix data or passed PID dests
            break;

        if(switchVal[eeData.mixData[idx].swtch])
            doMix(idx);
    }

    //pid loop
    calcPID();   //Calculate PID and post back to CMD


    //post/main mixer
    while (idx<MAX_MIXERS)
    {
        if((!eeData.mixData[idx].destCh)) //if no more mix data
            break;

        if(switchVal[eeData.mixData[idx].swtch])
            doMix(idx);
    }


    //set up outputs
    for(idx=0; idx<MAX_CHN; idx++)
        pwm_out_set(idx, outVal[idx + MAX_KPID]);
}
